//
// Created by de on 3/29/21.
//

#include "RosHandler.h"
#include "std_srvs/SetBool.h"
//#include "hirop_msgs/getForceRequest.h"
//#include "hirop_msgs/getForceCtlStatus.h"
#include "force_bridge/getForceCtlStatus.h"
#include "hirop_msgs/forceCtlDevStatus.h"
#include "halcon_bridge/HeightDetect.h"
RosHandler::RosHandler() {

}

RosHandler::RosHandler(ros::NodeHandle *node) {
    this->node = node;

    server_launchPiH = node->serviceClient<std_srvs::SetBool>("/app_PiH/launchDev");
    server_startPiH = node->serviceClient<std_srvs::SetBool>("/app_PiH/start");
    server_stopPiH = node->serviceClient<std_srvs::SetBool>("/app_PiH/stop");
    server_resetPiH = node->serviceClient<std_srvs::SetBool>("/app_PiH/reset");
    server_forcedresetPiH = node->serviceClient<std_srvs::SetBool>("app_PiH/Forcedreset");
    server_getStatusPiH = node->serviceClient<force_bridge::getForceCtlStatus>("/app_PiH/getStatus");
    server_testPiH = node->serviceClient<std_srvs::SetBool>("/app_PiH/test");
    VisionMeasureCli = node->serviceClient<halcon_bridge::HeightDetect>("/halcon_bridge/HeightDetect");

    forceStatus_sub=node->subscribe<hirop_msgs::forceCtlDevStatus>("forceCtlDevStatus",1,boost::bind(&RosHandler::forceStatus_Callback,this,_1));

    setEnableClient = node->serviceClient<hsr_rosi_device::SetEnableSrv>("/set_robot_enable");


}

bool RosHandler::sendDeviceCheck() {
    bool ret = true;
    ForceCtlStatus ControlStatus = getDeviceControlStateStatus().status;

    if(ControlStatus == Err || ControlStatus == Finish ||ControlStatus == Stop){
        ret = false;
    }

    return ret;
}

bool RosHandler::sendDeviceStart() {

    spdlog::info("开始服务");
    std_srvs::SetBool srv;
    srv.request.data = false;
    bool ret = server_startPiH.call(srv);

    return ret;
}

bool RosHandler::sendDeviceStop() {
    spdlog::info("停止服务");
    std_srvs::SetBool srv;
    return server_stopPiH.call(srv);}

bool RosHandler::sendDeviceInit() {
    std_srvs::SetBool srv;

    return server_launchPiH.call(srv);
}

ForceStateData RosHandler::getDeviceControlStateStatus() {
//    spdlog::info("获取服务");
    ForceStateData forceCommunData;
    force_bridge::getForceCtlStatus srv;
    bool ret = server_getStatusPiH.call(srv);
    assert(ret == true);
    int code = srv.response.enum_forceCtlStatus;
    forceCommunData.errCodeInfoData.Err_stringInfo=srv.response.forceCtlErrData.err_info;
    forceCommunData.errCodeInfoData.Err_level=srv.response.forceCtlErrData.Errdegree;
    forceCommunData.errCodeInfoData.errCode=ErrCode(srv.response.forceCtlErrData.ErrCode);

    forceCommunData.status = (ForceCtlStatus)code;
    forceCommunData.Info= srv.response.info;
    return forceCommunData;
}

void RosHandler::forceStatus_Callback(const hirop_msgs::forceCtlDevStatus::ConstPtr &msg) {
    std::vector<int> v_Error;
    v_Error.clear();
    v_Error.push_back(msg->forceSensorDevOk);
    v_Error.push_back(msg->robotDevConn);
//    v_Error.push_back(msg->robotDevEnable);//运行前有上下使能操作，暂时不一直检测
    v_Error.push_back(msg->rosnode_force_bridge);
    v_Error.push_back(msg->rosnode_forceCompensation);
    v_Error.push_back(msg->rosnode_PiHapp);
    v_Error.push_back(msg->robotDevErr+1);
    this->v_Error.clear();

    this->v_Error = v_Error;

}

ForceDeviceCommCode RosHandler::getDeviceCommuStatus() {
    ForceDeviceCommCode Dcode;
    for (int i = 0; i < v_Error.size(); ++i)
    {
        if(v_Error.at(i) == 0)
            Dcode.v_Error.push_back(i);
    }

    if(Dcode.v_Error.size() > 0)
        Dcode.Error_status = true;


    return Dcode;
}

bool RosHandler::sendDeviceReset() {
    spdlog::info("reset service");

    std_srvs::SetBool srv;
    return  server_resetPiH.call(srv);
}

bool RosHandler::sendDeviceForceReset() {
    spdlog::info("server_forcedresetPiH service");
    std_srvs::SetBool srv;
    return  server_forcedresetPiH.call(srv);
}

bool RosHandler::sendDeviceEnable() {
    hsr_rosi_device::SetEnableSrv srv;
    srv.request.enable=true;
    return setEnableClient.call(srv);
}

VisionMeasure RosHandler::sendVisionMeaDetect() {
    VisionMeasure visionMeasure;
    halcon_bridge::HeightDetect srv;
    VisionMeasureCli.call(srv);
    visionMeasure.Height = srv.response.distance;
    visionMeasure.CodeMessage = srv.response.code;

    return visionMeasure;
}



